#include "Rais_Car.h"

//Car - Methods
Car::Car(int halfPwrRange){
	if(halfPwrRange < 0) {
		if(halfPwrRange == R_INT16_MIN) halfPwrRange++;
		halfPwrRange =  -halfPwrRange;
	}
	this->halfPwrRange = halfPwrRange;
}

//DoubleWheelsCar - Methods
DoubleWheelsCar::DoubleWheelsCar(int halfPwrRange): Car(halfPwrRange){
	//nothing.
}

void DoubleWheelsCar::run(int pwr, int dir){
	if(this->dir == dir && this->pwr == pwr) return;
	CONSTRAIN(dir, -179, 180);
	CONSTRAIN(pwr, -halfPwrRange, halfPwrRange);
	
	if(dir >= 0)
		if(dir <= 90){
			runLWheel(pwr * left_isClockwise);
			runRWheel(pwr * (1 - dir / 45.0) * right_isClockwise);
		}else {
			runLWheel(pwr * (1 - (dir - 90) / 45.0) * left_isClockwise);
			runRWheel(-pwr * right_isClockwise);
		}
	else
		if(-dir <= 90){
			runLWheel(pwr * ((dir + 90) / 45.0 - 1) * left_isClockwise);
			runRWheel(pwr * right_isClockwise);
		}else {
			runLWheel(-pwr * left_isClockwise);
			runRWheel(pwr * ((dir + 180) / 45.0 - 1) * right_isClockwise);
		}
	this->dir = dir;
	this->pwr = pwr;
}

//DXMotorCar - Methods
DXMotorCar::DXMotorCar(): DoubleWheelsCar(255) {
	M1_isLWheel = true;
}

void DXMotorCar::run(int pwr, int dir){
	DoubleWheelsCar::run(pwr, dir);
}

void DXMotorCar::setWheels(bool L_R){
	M1_isLWheel = L_R;
}

DXMotorCar& DXMotorCar::getDXMotorCar(){
	static DXMotorCar car;
	return car;
}

void DXMotorCar::runLWheel(int pwr){
	M1_isLWheel ? M1.run(pwr) : M2.run(pwr);
}

void DXMotorCar::runRWheel(int pwr){
	M1_isLWheel ? M2.run(pwr) : M1.run(pwr);
}

int DXMotorCar::getLSpeed(){
	return M1_isLWheel ? M1.getSpeed() : M2.getSpeed();
}

int DXMotorCar::getRSpeed(){
	return M1_isLWheel ? M2.getSpeed() : M1.getSpeed();
}

DXMotorCar& dxcar = DXMotorCar::getDXMotorCar();

#undef CONSTRAIN
#undef R_INT16_MIN